/*
 * Pisces Graphics
 * Copyright (C) 2010 John Pritchard
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * CLASSPATH Exception
 * 
 * Linking this library statically or dynamically with other modules
 * is making a combined work based on this library. Thus, the terms
 * and conditions of the GNU General Public License cover the whole
 * combination.
 * 
 * As a special exception, the copyright holders of this library give
 * you permission to link this library with independent modules to
 * produce an executable, regardless of the license terms of these
 * independent modules, and to copy and distribute the resulting
 * executable under terms of your choice, provided that you also meet,
 * for each linked independent module, the terms and conditions of the
 * license of that module. An independent module is a module which is
 * not derived from or based on this library. If you modify this
 * library, you may extend this exception to your version of the
 * library, but you are not obligated to do so. If you do not wish to
 * do so, delete this exception statement from your version.
 * 
 * The CLASSPATH Exception is discussed briefly at
 * <http://www.gnu.org/software/classpath/license.html>.
 */
/*
 * Copyright 2007 Sun Microsystems, Inc.  All Rights Reserved.
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * This code is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License version 2 only, as
 * published by the Free Software Foundation.  Sun designates this
 * particular file as subject to the "Classpath" exception as provided
 * by Sun in the LICENSE file that accompanied this code.
 *
 * This code is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
 * version 2 for more details (a copy is included in the LICENSE file that
 * accompanied this code).
 *
 * You should have received a copy of the GNU General Public License version
 * 2 along with this work; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
 *
 * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
 * CA 95054 USA or visit www.sun.com if you need additional information or
 * have any questions.
 */
package pisces.m;

/**
 * A 4-element quaternion represented by double precision floating 
 * point x,y,z,w coordinates.  The quaternion is always normalized.
 *
 */
public class Quat 
    extends Tuple
{

    public double w;


    public Quat(double x, double y, double z, double w)
    {
        super();
        this.set(x,y,z,w);

    }
    public Quat(double[] q)
    {
        super();
        switch(q.length){
        case 2:
            this.set(q[0],q[1],0.0,0.0);
            break;
        case 3:
            this.set(q[0],q[1],q[2],0.0);
            break;
        case 4:
            this.set(q[0],q[1],q[2],q[3]);
            break;
        default:
            throw new IllegalArgumentException(String.valueOf(q.length));
        }
    }
    public Quat(Quat q)
    {
        super();
        this.set(q.x,q.y,q.z,q.w);
    }
    public Quat(Tuple t)
    {
        super();
        this.set(t.x,t.y,t.z,0.0);
    }
    /**
     * (0,0,0,0)
     */
    public Quat()
    {
        super();
    }


    /**
     * Sets the value of this quaternion to the conjugate of quaternion q1.
     * @param q1 the source vector
     */
    public final Quat conjugate(Quat q1)
    {
        this.x = -q1.x;
        this.y = -q1.y;
        this.z = -q1.z;
        this.w = q1.w;

        return this;
    }
    /**
     * Negate the value of of each of this quaternion's x,y,z coordinates 
     *  in place.
     */
    public final Quat conjugate()
    {
        this.x = -this.x;
        this.y = -this.y;
        this.z = -this.z;

        return this;
    }
    /**
     * Sets the value of this quaternion to the quaternion product of
     * quaternions q1 and q2 (this = q1 * q2).  
     * Note that this is safe for aliasing (e.g. this can be q1 or q2).
     * @param q1 the first quaternion
     * @param q2 the second quaternion
     */
    public final Quat mul(Quat q1, Quat q2)
    {
        if (this != q1 && this != q2) {

            this.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
            this.x = q1.w*q2.x + q2.w*q1.x + q1.y*q2.z - q1.z*q2.y;
            this.y = q1.w*q2.y + q2.w*q1.y - q1.x*q2.z + q1.z*q2.x;
            this.z = q1.w*q2.z + q2.w*q1.z + q1.x*q2.y - q1.y*q2.x;
        } 
        else {
            this.x = q1.w*q2.x + q2.w*q1.x + q1.y*q2.z - q1.z*q2.y;
            this.y = q1.w*q2.y + q2.w*q1.y - q1.x*q2.z + q1.z*q2.x;
            this.z = q1.w*q2.z + q2.w*q1.z + q1.x*q2.y - q1.y*q2.x;
            this.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
        }
        return this;
    }
    /**
     * Sets the value of this quaternion to the quaternion product of
     * itself and q1 (this = this * q1).  
     * @param q1 the other quaternion
     */
    public final Quat mul(Quat q1)
    {
        double w = this.w*q1.w - this.x*q1.x - this.y*q1.y - this.z*q1.z;
        double x = this.w*q1.x + q1.w*this.x + this.y*q1.z - this.z*q1.y;
        double y = this.w*q1.y + q1.w*this.y - this.x*q1.z + this.z*q1.x;
        double z = this.w*q1.z + q1.w*this.z + this.x*q1.y - this.y*q1.x;
        this.w = w;
        this.x = x;
        this.y = y;
        this.z = z;

        return this;
    } 
    /** 
     * Multiplies quaternion q1 by the inverse of quaternion q2 and places
     * the value into this quaternion.  The value of both argument quaternions 
     * is preservered (this = q1 * q2^-1).
     * @param q1 the first quaternion 
     * @param q2 the second quaternion
     */ 
    public final Quat mulInverse(Quat q1, Quat q2) 
    {   
        return this.mul(q1, new Quat(q2).inverse());
    }
    /**
     * Multiplies this quaternion by the inverse of quaternion q1 and places
     * the value into this quaternion.  The value of the argument quaternion
     * is preserved (this = this * q^-1).
     * @param q1 the other quaternion
     */
    public final Quat mulInverse(Quat q1)
    {  
        return this.mul(new Quat(q1).inverse());
    }
    /**
     * Sets the value of this quaternion to quaternion inverse of quaternion q1.
     * @param q1 the quaternion to be inverted
     */
    public final Quat inverse(Quat q1)
    {
        double norm = 1.0/(q1.w*q1.w + q1.x*q1.x + q1.y*q1.y + q1.z*q1.z);
        this.w =  norm*q1.w;
        this.x = -norm*q1.x;
        this.y = -norm*q1.y;
        this.z = -norm*q1.z;

        return this;
    }
    /**
     * Sets the value of this quaternion to the quaternion inverse of itself.
     */
    public final Quat inverse()
    {
        double norm = Z1(1.0/(this.w*this.w + this.x*this.x + this.y*this.y + this.z*this.z));
        this.w *=  norm;
        this.x *= -norm;
        this.y *= -norm;
        this.z *= -norm;
        return this;
    }
    /**
     * Sets the value of this quaternion to the normalized value
     * of quaternion q1.
     * @param q1 the quaternion to be normalized.
     */
    public final Quat set(double x, double y, double z, double w)
    {
        double norm = Z(x*x + y*y + z*z + w*w);

        if (norm > 0.0) {
            norm = 1.0/Math.sqrt(norm);
            this.x = norm*x;
            this.y = norm*y;
            this.z = norm*z;
            this.w = norm*w;
        }
        else {
            this.x =  0.0;
            this.y =  0.0;
            this.z =  0.0;
            this.w =  0.0;
        }
        return this;
    }
    /**
     * Normalizes the value of this quaternion in place.
     */
    public final Quat normalize()
    {
        double norm = Z(this.x*this.x + this.y*this.y + this.z*this.z + this.w*this.w);

        if (norm > 0.0) {
            norm = 1.0 / Math.sqrt(norm);
            this.x *= norm;
            this.y *= norm;
            this.z *= norm;
            this.w *= norm;
        }
        else {
            this.x =  0.0;
            this.y =  0.0;
            this.z =  0.0;
            this.w =  0.0;
        }
        return this;
    }
    /**
     * Sets the value of this quaternion to the rotational component of
     * the passed matrix.
     * @param m1 the matrix
     */
    public final Quat set(Matrix m1)
    {
        double ww = (m1.m00 + m1.m11 + m1.m22 + 1.0)/4.0;

        if (ww >= 0) {
            if (ww >= EPS_D2) {
                this.w = Math.sqrt(ww);
                ww = 0.25/this.w;
                this.x = (m1.m21 - m1.m12)*ww;
                this.y = (m1.m02 - m1.m20)*ww;
                this.z = (m1.m10 - m1.m01)*ww;
                return this;
            }
        } else {
            this.w = 0;
            this.x = 0;
            this.y = 0;
            this.z = 1;
            return this;
        }

        this.w = 0;
        ww = -0.5*(m1.m11 + m1.m22);
        if (ww >= 0) {
            if (ww >= EPS_D2){
                this.x =  Math.sqrt(ww);
                ww = 0.5/this.x;
                this.y = m1.m10*ww;
                this.z = m1.m20*ww;
                return this;
            }
        } else {
            this.x = 0;
            this.y = 0;
            this.z = 1;
            return this;
        }
     
        this.x = 0.0;
        ww = 0.5*(1.0 - m1.m22);
        if (ww >= EPS_D2) {
            this.y =  Math.sqrt(ww);
            this.z = m1.m21/(2.0*this.y);
            return this;
        }
        else {
            this.y =  0;
            this.z =  1;

            return this;
        }
    }
    /**
     * Sets the value of this quaternion to the equivalent rotation
     * of the AxisAngle argument.
     * @param a  the AxisAngle to be emulated
     */
    public final Quat set(AxisAngle a)
    {
        /*
         * Quat = cos(theta/2) + sin(theta/2)(roation_axis) 
         */
        double amag = Math.sqrt( a.x*a.x + a.y*a.y + a.z*a.z);
        if ( 0.0 == Z(amag)){
            w = 0.0;
            x = 0.0;
            y = 0.0;
            z = 0.0;
        }
        else {  
            amag = 1.0/amag; 
            double mag = Math.sin(a.angle/2.0);
            w = Math.cos(a.angle/2.0);
            x = a.x*amag*mag;
            y = a.y*amag*mag;
            z = a.z*amag*mag;
        }
        return this;
    }    
    /**
     *  Performs a great circle interpolation between this quaternion
     *  and the quaternion parameter and places the result into this
     *  quaternion.
     *  @param q1  the other quaternion
     *  @param alpha  the alpha interpolation parameter
     */
    public final Quat interpolate(Quat q1, double alpha) {
        // From "Advanced Animation and Rendering Techniques"
        // by Watt and Watt pg. 364, function as implemented appeared to be
        // incorrect.  Fails to choose the same quaternion for the double
        // covering. Resulting in change of direction for rotations.
        // Fixed function to negate the first quaternion in the case that the
        // dot product of q1 and this is negative. Second case was not needed.
        double dot,s1,s2,om,sinom;

        dot = x*q1.x + y*q1.y + z*q1.z + w*q1.w;

        if ( dot < 0 ) {
            // negate quaternion
            q1.x = -q1.x;  q1.y = -q1.y;  q1.z = -q1.z;  q1.w = -q1.w;
            dot = -dot;
        }

        if ( (1.0 - dot) > EPS ) {
            om = Math.acos(dot);
            sinom = Math.sin(om);
            s1 = Math.sin((1.0-alpha)*om)/sinom;
            s2 = Math.sin( alpha*om)/sinom;
        } else{
            s1 = 1.0 - alpha;
            s2 = alpha;
        }

        w = s1*w + s2*q1.w;
        x = s1*x + s2*q1.x;
        y = s1*y + s2*q1.y;
        z = s1*z + s2*q1.z;
        return this;
    }
    /**
     *  Performs a great circle interpolation between quaternion q1
     *  and quaternion q2 and places the result into this quaternion.
     *  @param q1  the first quaternion
     *  @param q2  the second quaternion
     *  @param alpha  the alpha interpolation parameter
     */
    public final Quat interpolate(Quat q1, Quat q2, double alpha) {
        // From "Advanced Animation and Rendering Techniques"
        // by Watt and Watt pg. 364, function as implemented appeared to be
        // incorrect.  Fails to choose the same quaternion for the double
        // covering. Resulting in change of direction for rotations.
        // Fixed function to negate the first quaternion in the case that the
        // dot product of q1 and this is negative. Second case was not needed.
        double dot,s1,s2,om,sinom;

        dot = q2.x*q1.x + q2.y*q1.y + q2.z*q1.z + q2.w*q1.w;

        if ( dot < 0 ) {
            // negate quaternion
            q1.x = -q1.x;  q1.y = -q1.y;  q1.z = -q1.z;  q1.w = -q1.w;
            dot = -dot;
        }

        if ( (1.0 - dot) > EPS ) {
            om = Math.acos(dot);
            sinom = Math.sin(om);
            s1 = Math.sin((1.0-alpha)*om)/sinom;
            s2 = Math.sin( alpha*om)/sinom;
        } else{
            s1 = 1.0 - alpha;
            s2 = alpha;
        }
        w = s1*q1.w + s2*q2.w;
        x = s1*q1.x + s2*q2.x;
        y = s1*q1.y + s2*q2.y;
        z = s1*q1.z + s2*q2.z;
        return this;
    }
    public Quat clone(){
        return (Quat)super.clone();
    }
}
